/**
 * @file hrt_call.c
 * 
 * High Resolution Timer implementation for FreeRTOS
 */

#include "px4_platform_common/hrt_call.h"
#include "FreeRTOS.h"
#include "task.h"
#include "timers.h"
#include <string.h>

/* Static variables */
static volatile uint64_t hrt_base_time_us = 0;  /**< Base time in microseconds */
static volatile uint32_t hrt_last_tick = 0;     /**< Last tick count for overflow detection */

/**
 * @brief Get absolute time in microseconds
 */
hrt_abstime hrt_absolute_time(void)
{
    /* Get current tick count */
    uint32_t current_tick = xTaskGetTickCount();
    
    /* Calculate elapsed ticks (handles overflow) */
    uint32_t elapsed_ticks = current_tick - hrt_last_tick;
    
    /* Convert ticks to microseconds (1 tick = 1ms = 1000us) */
    uint64_t elapsed_us = (uint64_t)elapsed_ticks * 1000ULL;
    
    /* Update base time and last tick */
    taskENTER_CRITICAL();
    hrt_base_time_us += elapsed_us;
    hrt_last_tick = current_tick;
    uint64_t result = hrt_base_time_us;
    taskEXIT_CRITICAL();
    
    return result;
}

/**
 * @brief Store absolute time in an interrupt-safe manner
 */
void hrt_store_absolute_time(volatile hrt_abstime *t)
{
    taskENTER_CRITICAL();
    *t = hrt_absolute_time();
    taskEXIT_CRITICAL();
}

/**
 * @brief Initialize the HRT subsystem
 */
void hrt_init(void)
{
    /* Initialize base time */
    hrt_base_time_us = 0;
    hrt_last_tick = xTaskGetTickCount();
}

/**
 * @brief Timer callback wrapper
 *
 * This function is called by FreeRTOS when a timer expires.
 * It calls the user's callback function.
 */
static void hrt_timer_callback(TimerHandle_t xTimer)
{
    hrt_call_t *entry = (hrt_call_t *)pvTimerGetTimerID(xTimer);

    if (entry != NULL && entry->callout != NULL) {
        /* Call user callback */
        entry->callout(entry->arg);

        /* For one-shot timers, mark as inactive */
        if (entry->period == 0) {
            entry->active = false;
        } else {
            /* For periodic timers, if this is the first call (one-shot mode),
             * switch to auto-reload mode for subsequent calls */
            if (uxTimerGetReloadMode(xTimer) == pdFALSE) {
                /* Stop the timer */
                xTimerStop(xTimer, 0);

                /* Switch to auto-reload mode */
                vTimerSetReloadMode(xTimer, pdTRUE);

                /* Set the period */
                TickType_t period_ticks = pdMS_TO_TICKS((entry->period + 999) / 1000);
                xTimerChangePeriod(xTimer, period_ticks, 0);

                /* Restart the timer */
                xTimerStart(xTimer, 0);
            }
        }
    }
}

/**
 * @brief Initialize a callout structure
 */
void hrt_call_init(hrt_call_t *entry)
{
    if (entry == NULL) {
        return;
    }
    
    memset(entry, 0, sizeof(hrt_call_t));
    
    /* Create a FreeRTOS timer (initially stopped) */
    entry->timer = xTimerCreate(
        "hrt_timer",                    /* Timer name */
        pdMS_TO_TICKS(1),               /* Initial period (will be updated) */
        pdFALSE,                        /* One-shot by default */
        (void *)entry,                  /* Timer ID = pointer to entry */
        hrt_timer_callback              /* Callback function */
    );
    
    entry->active = false;
}

/**
 * @brief Internal function to schedule a timer
 */
static void hrt_call_internal(hrt_call_t *entry, hrt_abstime deadline, hrt_abstime interval,
                               hrt_callout callout, void *arg)
{
    if (entry == NULL || entry->timer == NULL) {
        return;
    }
    
    /* Cancel existing timer if active */
    if (entry->active) {
        xTimerStop(entry->timer, 0);
        entry->active = false;
    }
    
    /* Update entry */
    entry->deadline = deadline;
    entry->period = interval;
    entry->callout = callout;
    entry->arg = arg;
    
    /* Calculate delay in ticks */
    hrt_abstime now = hrt_absolute_time();
    hrt_abstime delay_us = (deadline > now) ? (deadline - now) : 0;
    TickType_t delay_ticks = pdMS_TO_TICKS((delay_us + 999) / 1000);  /* Round up */

    /* Ensure minimum delay of 1 tick */
    if (delay_ticks == 0) {
        delay_ticks = 1;
    }

    /* For periodic timers, start with one-shot mode for the initial delay,
     * then switch to auto-reload in the callback */
    if (interval > 0) {
        /* Start as one-shot timer with initial delay */
        vTimerSetReloadMode(entry->timer, pdFALSE);
        xTimerChangePeriod(entry->timer, delay_ticks, 0);
    } else {
        /* One-shot timer */
        vTimerSetReloadMode(entry->timer, pdFALSE);
        xTimerChangePeriod(entry->timer, delay_ticks, 0);
    }

    /* Start timer */
    xTimerStart(entry->timer, 0);
    entry->active = true;
}

/**
 * @brief Schedule a one-shot callback after a delay
 */
void hrt_call_after(hrt_call_t *entry, hrt_abstime delay, hrt_callout callout, void *arg)
{
    hrt_abstime deadline = hrt_absolute_time() + delay;
    hrt_call_internal(entry, deadline, 0, callout, arg);
}

/**
 * @brief Schedule a callback at an absolute time
 */
void hrt_call_at(hrt_call_t *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
{
    hrt_call_internal(entry, calltime, 0, callout, arg);
}

/**
 * @brief Schedule a periodic callback
 */
void hrt_call_every(hrt_call_t *entry, hrt_abstime delay, hrt_abstime interval,
                    hrt_callout callout, void *arg)
{
    hrt_abstime deadline = hrt_absolute_time() + delay;
    hrt_call_internal(entry, deadline, interval, callout, arg);
}

/**
 * @brief Cancel a scheduled callback
 */
void hrt_cancel(hrt_call_t *entry)
{
    if (entry == NULL || entry->timer == NULL) {
        return;
    }

    if (entry->active) {
        xTimerStop(entry->timer, portMAX_DELAY);
        entry->active = false;
        entry->deadline = 0;
        entry->period = 0;
    }
}

/**
 * @brief Check if a callout has been invoked
 */
bool hrt_called(hrt_call_t *entry)
{
    if (entry == NULL) {
        return true;
    }

    /* For repeating callouts, always return false */
    if (entry->period > 0) {
        return false;
    }

    /* For one-shot timers, return true if not active */
    return !entry->active;
}

